#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <opencv2/opencv.hpp>
#include <chrono>

using namespace std::chrono_literals;

class ImagePublisher : public rclcpp::Node
{
public:
    ImagePublisher() : Node("image_publisher"), count_(0)
    {
        publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image_topic", 10);
        timer_ = this->create_wall_timer(
            500ms, std::bind(&ImagePublisher::publish_image, this));
        
        RCLCPP_INFO(this->get_logger(), "图像发布者已启动");
    }

private:
    void publish_image()
    {
        // 创建测试图像
        cv::Mat image(480, 640, CV_8UC3, cv::Scalar(50, 50, 50));
        
        // 在图像上添加变化的文本
        std::string text = "ROS2 Image " + std::to_string(count_++);
        cv::putText(image, text, cv::Point(100, 240), 
                   cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 255), 2);
        
        // 添加一个移动的圆
        int radius = 50;
        int x = 320 + static_cast<int>(100 * sin(count_ * 0.1));
        int y = 240 + static_cast<int>(100 * cos(count_ * 0.1));
        cv::circle(image, cv::Point(x, y), radius, cv::Scalar(255, 0, 0), -1);
        
        // 转换为ROS图像消息
        auto msg = std::make_unique<sensor_msgs::msg::Image>();
        msg->header.stamp = this->now();
        msg->header.frame_id = "camera_frame";
        msg->height = image.rows;
        msg->width = image.cols;
        msg->encoding = "bgr8";
        msg->is_bigendian = false;
        msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(image.step);
        msg->data.assign(image.datastart, image.dataend);
        
        publisher_->publish(std::move(msg));
        RCLCPP_DEBUG(this->get_logger(), "发布图像 %d", count_);
    }
    
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
    size_t count_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImagePublisher>());
    rclcpp::shutdown();
    return 0;
}